/*
The MIT License
Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE

This file is part of embeddedRTPS.

Author: i11 - Embedded Software, RWTH Aachen University
*/

#include "rtps/entities/Participant.h"

#include "rtps/entities/Reader.h"
#include "rtps/entities/Writer.h"
#include "rtps/messages/MessageReceiver.h"
#include "rtps/utils/Log.h"

#if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE
#define PARTICIPANT_LOG(...)                                                   \
  if (true) {                                                                  \
    printf("[Participant] ");                                                  \
    printf(__VA_ARGS__);                                                       \
    printf("\n");                                                              \
  }
#else
#define PARTICIPANT_LOG(...) //
#endif

void rtps_Participant_Construct(struct rtps_Participant *participant, 
                      const GuidPrefix_t *guidPrefix, ParticipantId_t *participantId) {
  participant->m_hasBuilInEndpoints = false;
  participant->m_numWriters = 0;
  participant->m_numReaders = 0;
  participant->m_nextUserEntityId[0] = 0;
  participant->m_nextUserEntityId[1] = 0;
  participant->m_nextUserEntityId[2] = 1;
  if (guidPrefix)
    participant->m_guidPrefix = *guidPrefix;
  else
    participant->m_guidPrefix = GUIDPREFIX_UNKNOWN;

  if (participantId)
    participant->m_participantId = *participantId;
  else
    participant->m_participantId = PARTICIPANT_ID_INVALID;

  participant->m_receiver.mp_part = participant;

	participant->m_remoteParticipants = rtps_MemoryPool_init(sizeof(ParticipantProxyData), SPDP_MAX_NUMBER_FOUND_PARTICIPANTS);
  if (sys_mutex_new(&participant->m_mutex) != ERR_OK) {
    while (1)
      ;
  }
}

void rtps_Participant_Destruct(struct rtps_Participant *participant) {
  rtps_SPDPAgent_stop(&participant->m_spdpAgent);
  rtps_SEDPAgent_deinit(&participant->m_sedpAgent);
  rtps_MemoryPool_deinit(participant->m_remoteParticipants);
}

void rtps_Participant_reuse(struct rtps_Participant *participant, const GuidPrefix_t *guidPrefix,
                        ParticipantId_t participantId) {
  participant->m_guidPrefix = *guidPrefix;
  participant->m_participantId = participantId;
}

bool rtps_Participant_isValid(struct rtps_Participant *participant) {
  return participant->m_participantId != PARTICIPANT_ID_INVALID;
}

//std::array<uint8_t, 3> rtps_Participant_getNextUserEntityKey(struct Participant *participant) { //fix me by liudi
uint8_t *rtps_Participant_getNextUserEntityKey(struct rtps_Participant *participant) {
  ++(participant->m_nextUserEntityId)[2];
  if (participant->m_nextUserEntityId[2] == 0) {
    ++(participant->m_nextUserEntityId)[1];
    if (participant->m_nextUserEntityId[1] == 0) {
      ++participant->m_nextUserEntityId[0];

    }
  }
  return participant->m_nextUserEntityId;
}

bool rtps_Participant_registerOnNewPublisherMatchedCallback(struct rtps_Participant *participant,
    void (*callback)(void *arg), void *args) {
  if (!participant->m_hasBuilInEndpoints) {
    return false;
  }

  rtps_SEDPAgent_registerOnNewPublisherMatchedCallback(&participant->m_sedpAgent, callback, args);
  return true;
}

bool rtps_Participant_registerOnNewSubscriberMatchedCallback(struct rtps_Participant *participant,
    void (*callback)(void *arg), void *args) {
  if (!participant->m_hasBuilInEndpoints) {
    return false;
  }

  rtps_SEDPAgent_registerOnNewSubscriberMatchedCallback(&participant->m_sedpAgent, callback, args);
  return true;
}

struct rtps_Writer *rtps_Participant_addWriter(struct rtps_Participant *participant, struct rtps_Writer *pWriter) {
  if (pWriter != NULL && participant->m_numWriters != sizeof(participant->m_writers)/sizeof(Writer *)) {
    participant->m_writers[participant->m_numWriters++] = pWriter;
    if (participant->m_hasBuilInEndpoints) {
      rtps_SEDPAgent_addWriter(&participant->m_sedpAgent, pWriter);
    }
    return pWriter;
  } else {
    return NULL;
  }
}

bool rtps_Participant_isWritersFull(struct rtps_Participant *pParticipant) {
  return pParticipant->m_numWriters == sizeof(pParticipant->m_writers)/sizeof(struct rtps_Writer *); 
}

struct rtps_Reader *rtps_Participant_addReader(struct rtps_Participant *pParticipant, struct rtps_Reader *pReader) {
  if (pReader != NULL && pParticipant->m_numReaders != sizeof(pParticipant->m_readers)/sizeof(struct rtps_Reader *)) {
    pParticipant->m_readers[pParticipant->m_numReaders++] = pReader;
    if (pParticipant->m_hasBuilInEndpoints) {
      rtps_SEDPAgent_addReader(&pParticipant->m_sedpAgent, pReader);
    }
    return pReader;
  } else {
    return NULL;
  }
}

bool rtps_Participant_isReadersFull(struct rtps_Participant *pParticipant) {
  return pParticipant->m_numReaders == sizeof(pParticipant->m_readers)/sizeof(Reader *);
}

struct rtps_Writer *rtps_Participant_getWriter(struct rtps_Participant *pParticipant, EntityId_t id) {
  for (uint8_t i = 0; i < pParticipant->m_numWriters; ++i) {
    if (0 == memcmp(&pParticipant->m_writers[i]->m_attributes.endpointGuid.entityId, &id, sizeof(EntityId_t))) {
      return pParticipant->m_writers[i];
    }
  }
  return NULL;
}

struct rtps_Reader *rtps_Participant_getReader(struct rtps_Participant *pParticipant, EntityId_t id) {
  for (uint8_t i = 0; i < pParticipant->m_numReaders; ++i) {
    if (0 == memcmp(&(pParticipant->m_readers[i]->m_attributes.endpointGuid.entityId), &id, sizeof(EntityId_t))) {
      return pParticipant->m_readers[i];
    }
  }
  return NULL;
}

struct rtps_Reader *rtps_Participant_getReaderByWriterId(struct rtps_Participant *pParticipant, const Guid_t *guid) {
  for (uint8_t i = 0; i < pParticipant->m_numReaders; ++i) {
    if (rtps_Reader_knowWriterId(pParticipant->m_readers[i], guid)) {
      return pParticipant->m_readers[i];
    }
  }
  return NULL;
}

struct rtps_Writer *
rtps_Participant_getMatchingWriter(struct rtps_Participant *pParticipant, const TopicData *readerTopicData) {
  for (uint8_t i = 0; i < pParticipant->m_numWriters; ++i) {
    if (rtps_TopicData_matchesTopicOf(&pParticipant->m_writers[i]->m_attributes, readerTopicData) &&
        (readerTopicData->reliabilityKind == BEST_EFFORT ||
         pParticipant->m_writers[i]->m_attributes.reliabilityKind ==
             RELIABLE)) {
      return pParticipant->m_writers[i];
    }
  }
  return NULL;
}

struct rtps_Reader *
rtps_Participant_getMatchingReader(struct rtps_Participant *pParticipant, const TopicData *writerTopicData) {
  for (uint8_t i = 0; i < pParticipant->m_numReaders; ++i) {
    if (rtps_TopicData_matchesTopicOf(&pParticipant->m_readers[i]->m_attributes, writerTopicData) &&
        (writerTopicData->reliabilityKind == RELIABLE ||
         pParticipant->m_readers[i]->m_attributes.reliabilityKind ==
             BEST_EFFORT)) {
      return pParticipant->m_readers[i];
    }
  }
  return NULL;
}

struct rtps_Writer *rtps_Participant_getMatchingWriter_compressed(struct rtps_Participant *pParticipant,
    const TopicDataCompressed *readerTopicData) {
  for (uint8_t i = 0; i < pParticipant->m_numWriters; ++i) {
    if (rtps_TopicDataCompressed_matchesTopicOf(readerTopicData, &pParticipant->m_writers[i]->m_attributes) &&
        (readerTopicData->reliabilityKind == BEST_EFFORT ||
         pParticipant->m_writers[i]->m_attributes.reliabilityKind ==
             RELIABLE)) {
      return pParticipant->m_writers[i];
    }
  }
  return NULL;
}
struct rtps_Reader *rtps_Participant_getMatchingReader_compressed(struct rtps_Participant *pParticipant,
    const TopicDataCompressed *writerTopicData) {
  for (uint8_t i = 0; i < pParticipant->m_numReaders; ++i) {
    if (rtps_TopicDataCompressed_matchesTopicOf(writerTopicData, &pParticipant->m_readers[i]->m_attributes) &&
        (writerTopicData->reliabilityKind == RELIABLE ||
         pParticipant->m_readers[i]->m_attributes.reliabilityKind ==
             BEST_EFFORT)) {
      return pParticipant->m_readers[i];
    }
  }
  return NULL;
}


bool rtps_Participant_addNewRemoteParticipant(struct rtps_Participant *pParticipant,
    const ParticipantProxyData *remotePart) {
  rtps_Lock(&pParticipant->m_mutex);
  bool retval = rtps_MemoryPool_add(pParticipant->m_remoteParticipants, (void *)remotePart);
  rtps_Unlock(&pParticipant->m_mutex);
  return retval;
}

static bool participantProxyData_cmp_guidPrefix(void *key, void *data) {
  const GuidPrefix_t *prefix = (const GuidPrefix_t *)key;
  const ParticipantProxyData *proxy = (const ParticipantProxyData *)data;
  return (0 == memcmp(&proxy->m_guid.prefix, prefix, sizeof(GuidPrefix_t)));
}

bool rtps_Participant_removeRemoteParticipant(struct rtps_Participant *pParticipant, const GuidPrefix_t *prefix) {
  rtps_Participant_removeAllEntitiesOfParticipant(pParticipant, prefix);
  return rtps_MemoryPool_remove(pParticipant->m_remoteParticipants, participantProxyData_cmp_guidPrefix, (void *)prefix);
}

void rtps_Participant_removeAllEntitiesOfParticipant(struct rtps_Participant *pParticipant, const GuidPrefix_t *prefix) {
  for (uint8_t i = 0; i < pParticipant->m_numWriters; i++) {
    //fix me, it's dangerous to convert to a bigger struct
    if (pParticipant->m_writers[i]->m_bstateful)
        rtps_StatefulWriter_removeReaderOfParticipant((struct rtps_StatefulWriter *)pParticipant->m_writers[i], prefix);
    else
        rtps_StatelessWriter_removeReaderOfParticipant((struct rtps_StatelessWriter *)pParticipant->m_writers[i], prefix);
  }

  for (uint8_t i = 0; i < pParticipant->m_numReaders; i++) {
    //fix me, it's dangerous to convert to a bigger struct
    if (pParticipant->m_readers[i]->m_bstateful)
      rtps_StatefulReader_removeWriterOfParticipant((struct rtps_StatefulReader *)pParticipant->m_readers[i], prefix);
    else
      rtps_StatelessReader_removeWriterOfParticipant((struct rtps_StatelessReader *)pParticipant->m_readers[i], prefix);
  }
}

/*need to use rtps_Participant_findRemoteParticipant_free free ParticipantProxyData*/
ParticipantProxyData *
rtps_Participant_findRemoteParticipant(struct rtps_Participant *pParticipant, const GuidPrefix_t *prefix) {
  struct rtps_ParticipantProxyData *retval =  malloc(sizeof(struct rtps_ParticipantProxyData));
  bool isFind;
  rtps_Lock(&pParticipant->m_mutex);
  isFind = rtps_MemoryPool_getElement(pParticipant->m_remoteParticipants, 
                  participantProxyData_cmp_guidPrefix, (void *)prefix, retval, sizeof(struct rtps_ParticipantProxyData));
  if (!isFind) {
    rtps_Unlock(&pParticipant->m_mutex);
    free(retval);
    return NULL;
  }
  rtps_Unlock(&pParticipant->m_mutex);
  return retval;
}

void rtps_Participant_findRemoteParticipant_free(ParticipantProxyData *proxyData) {
  if (!proxyData)
    return;
  free(proxyData);
  return;
}

void rtps_Participant_refreshRemoteParticipantLiveliness(struct rtps_Participant *pParticipant,
    const GuidPrefix_t *prefix) {
  struct rtps_ParticipantProxyData *proxyData;
  bool retval;

  rtps_Lock(&pParticipant->m_mutex);
  retval = rtps_MemoryPool_getElement_not_safe(pParticipant->m_remoteParticipants, participantProxyData_cmp_guidPrefix,
                   (void *)prefix, (void **)&proxyData);
  if (retval)
    rtps_ParticipantProxyData_onAliveSignal(proxyData);
  rtps_Unlock(&pParticipant->m_mutex);
}

bool rtps_Participant_hasReaderWithMulticastLocator(struct rtps_Participant *pParticipant, ip4_addr_t address) {
  for (uint8_t i = 0; i < pParticipant->m_numReaders; i++) {
    if (rtps_Locator_isSameAddress(&(pParticipant->m_readers[i]->m_attributes.multicastLocator), &address)) {
      return true;
    }
  }
  return false;
}

uint32_t rtps_Participant_getRemoteParticipantCount(struct rtps_Participant *pParticipant) {
  rtps_Lock(&pParticipant->m_mutex);
  uint32_t retval = rtps_MemoryPool_getNumElements(pParticipant->m_remoteParticipants);
  rtps_Unlock(&pParticipant->m_mutex);
  return retval;
}

MessageReceiver *rtps_Participant_getMessageReceiver(struct rtps_Participant *pParticipant) {
  return &pParticipant->m_receiver;
}

void rtps_Participant_addHeartbeat(struct rtps_Participant *pParticipant, GuidPrefix_t sourceGuidPrefix) {
  ParticipantProxyData *proxyData;
  bool isHaveRemote;
  rtps_Lock(&pParticipant->m_mutex);
  isHaveRemote = rtps_MemoryPool_getElement_not_safe(pParticipant->m_remoteParticipants, participantProxyData_cmp_guidPrefix, 
                         &sourceGuidPrefix, (void **)&proxyData);
  if (isHaveRemote)
    rtps_ParticipantProxyData_onAliveSignal(proxyData);
  rtps_Unlock(&pParticipant->m_mutex);
}

bool rtps_Participant_checkAndResetHeartbeats(struct rtps_Participant *pParticipant) {
  ParticipantProxyData *proxyData;
  uint32_t offset;
  uint8_t *ptmp, *pblk, *pdata;

  rtps_Lock(&pParticipant->m_mutex);
  PARTICIPANT_LOG("Have %u remote participants\n",
                  (unsigned int)rtps_MemoryPool_getNumElements(pParticipant->m_remoteParticipants));

  RTPS_MEMORYPOOL_ITER(pParticipant->m_remoteParticipants, offset, ptmp, pblk, pdata) {
    proxyData = (ParticipantProxyData *)pdata;
    PARTICIPANT_LOG("remote participant age = %u\n",
                    (unsigned int)rtps_ParticipantProxyData_getAliveSignalAgeInMilliseconds(proxyData));//fix me by liudi
    if (rtps_ParticipantProxyData_isAlive(proxyData)) {
      PARTICIPANT_LOG("remote participant is alive\n");
      continue;
    }
    PARTICIPANT_LOG("!!! REMOVING PARTICIPANT !!!\n");
    bool success = rtps_Participant_removeRemoteParticipant(pParticipant, &(proxyData->m_guid.prefix));
    if (!success) {
      rtps_Unlock(&pParticipant->m_mutex);
      return false;
    }
  }
  rtps_Unlock(&pParticipant->m_mutex);
  return true;
}

struct rtps_SPDPAgent *rtps_Participant_getSPDPAgent(rtps_Participant *pParticipant) {
   return &pParticipant->m_spdpAgent; 
}

void rtps_Participant_addBuiltInEndpoints(rtps_Participant *pParticipant, BuiltInEndpoints *pEndpoints) {
  pParticipant->m_hasBuilInEndpoints = true;
  rtps_SPDPAgent_init(&pParticipant->m_spdpAgent, pParticipant, pEndpoints);
  rtps_SEDPAgent_init(&pParticipant->m_sedpAgent, pParticipant, pEndpoints);

  // This needs to be done after initializing the agents
  rtps_Participant_addWriter(pParticipant, (struct rtps_Writer *)pEndpoints->spdpWriter);
  rtps_Participant_addReader(pParticipant, (struct rtps_Reader *)pEndpoints->spdpReader);
  rtps_Participant_addWriter(pParticipant, (struct rtps_Writer *)pEndpoints->sedpPubWriter);
  rtps_Participant_addReader(pParticipant, (struct rtps_Reader *)pEndpoints->sedpPubReader);
  rtps_Participant_addWriter(pParticipant, (struct rtps_Writer *)pEndpoints->sedpSubWriter);
  rtps_Participant_addReader(pParticipant, (struct rtps_Reader *)pEndpoints->sedpSubReader);
}

void rtps_Participant_newMessage(struct rtps_Participant *pParticipant, const uint8_t *data, DataSize_t size) {
  rtps_MessageReceiver_processMessage((uint8_t *)data, size, &pParticipant->m_receiver);
}
